
#include "stdafx.h"

#include "SensorDataReaderZhou.h"
#include "GlobalAppState.h"
#include "MatrixConversion.h"

#ifdef SENSOR_DATA_READER

#include "sensorData/sensorData.h"

#include <algorithm>
#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <string>

#include <conio.h>

SensorDataReaderZhou::SensorDataReaderZhou()
{
	m_numFrames = 0;
	m_currFrame = 0;
	m_bHasColorData = false;

	m_sensorData = NULL;
	m_sensorDataCache = NULL;
}

SensorDataReaderZhou::SensorDataReaderZhou(int x, int nFrames, int startFrame)
{
	m_numFrames = nFrames;

	m_currFrame = 0;
	m_startFrame = startFrame;
	m_bHasColorData = false;

	m_sensorData = NULL;
	m_sensorDataCache = NULL;
	m_type = x;
}

SensorDataReaderZhou::~SensorDataReaderZhou()
{
	releaseData();
}


HRESULT SensorDataReaderZhou::createFirstConnected()
{
	releaseData();

	std::string filename = GlobalAppState::get().s_binaryDumpSensorFile[0];

	std::cout << "Start loading binary dump... ";
	m_sensorData = new SensorDataZhou;

	if (m_type == 0)
		m_sensorData->loadFromFile(filename, m_numFrames);
	if (m_type == 1)
		m_sensorData->loadFromFileObject(filename, m_numFrames, m_startFrame);
	std::cout << "DONE!" << std::endl;
	std::cout << *m_sensorData << std::endl;

	//std::cout << "depth intrinsics:" << std::endl;
	//std::cout << m_sensorData->m_calibrationDepth.m_intrinsic << std::endl;
	//std::cout << "color intrinsics:" << std::endl;
	//std::cout << m_sensorData->m_calibrationColor.m_intrinsic << std::endl;

	std::cout << m_sensorData->m_depthWidth << " " << m_sensorData->m_depthHeight << " " << m_sensorData->m_colorWidth <<" "<< m_sensorData->m_colorHeight << std::endl;

	RGBDSensor::init(m_sensorData->m_depthWidth, m_sensorData->m_depthHeight, std::max(m_sensorData->m_colorWidth, 1u), std::max(m_sensorData->m_colorHeight, 1u), 1);
	initializeDepthIntrinsics(m_sensorData->m_calibrationDepth.m_intrinsic(0, 0), m_sensorData->m_calibrationDepth.m_intrinsic(1, 1), m_sensorData->m_calibrationDepth.m_intrinsic(0, 2), m_sensorData->m_calibrationDepth.m_intrinsic(1, 2));
	initializeColorIntrinsics(m_sensorData->m_calibrationColor.m_intrinsic(0, 0), m_sensorData->m_calibrationColor.m_intrinsic(1, 1), m_sensorData->m_calibrationColor.m_intrinsic(0, 2), m_sensorData->m_calibrationColor.m_intrinsic(1, 2));

	//Matrix3f R; R.setIdentity(); Vector3f t; t.setZero();
	//initializeColorExtrinsics(R, t);

	//R(0, 0) = 9.9991741106823473e-001; R(0, 1) = 3.0752530258331304e-003; R(0, 2) = -1.2478536028949385e-002;
	//R(1, 0) = -3.0607678272497924e-003; R(1, 1) = 9.9999461994140826e-001; R(1, 2) = 1.1797408808971066e-003;
	//R(2, 0) = 1.2482096895408091e-002; R(2, 1) = -1.1414495457493831e-003; R(2, 2) = 9.9992144408949846e-001;

	//t[0] = -2.5331974929667012e+001;  t[1] = 6.1798287248283634e-001; t[2] = 3.8510108109251804e+000;
	//t[0] /= 1000.0f; t[1] /= 1000.0f; t[2] /= 1000.0f;

	//initializeDepthExtrinsics(R, t);

	initializeDepthExtrinsics(m_sensorData->m_calibrationDepth.m_extrinsic);
	initializeColorExtrinsics(m_sensorData->m_calibrationColor.m_extrinsic);


	m_numFrames = (unsigned int)m_sensorData->m_frames.size();


	if (m_numFrames > 0 && m_sensorData->m_frames[0].getColorCompressed()) {
		m_bHasColorData = true;
	}
	else {
		m_bHasColorData = false;
	}

	const unsigned int cacheSize = 10;
	m_sensorDataCache = new RGBDFrameCacheRead(m_sensorData, cacheSize);

	return S_OK;
}

HRESULT SensorDataReaderZhou::processDepth()
{
	//if (m_currFrame >= m_numFrames)
	//{
	//	GlobalAppState::get().s_playData = false;
	//	//std::cout << "binary dump sequence complete - press space to run again" << std::endl;
	//	stopReceivingFrames();
	//	std::cout << "binary dump sequence complete - stopped receiving frames" << std::endl;
	//	m_currFrame = 0;
	//}
	//if (m_currFrame >= m_numFrames)
	//{
	//	
	//	int da = m_currFrame / m_numFrames;
	//	//std::cout << "binary dump sequence complete - press space to run again" << std::endl;

	//}
	if (m_sensorDataCache->getCheck()) GlobalAppState::get().s_integrationEnabled = false;

	if (GlobalAppState::get().s_playData) {

		float* depth = getDepthFloat();

		//TODO check why the frame cache is not used?
	    RGBDFrameCacheRead::FrameState frameState = m_sensorDataCache->getNextOurs();
		//ml::SensorData::RGBDFrameCacheRead::FrameState frameState;
		//frameState.m_colorFrame = m_sensorData->decompressColorAlloc(m_currFrame);
		//frameState.m_depthFrame = m_sensorData->decompressDepthAlloc(m_currFrame);
		//if (frameState.m_colorFrame == NULL)
		//	printf("WHAT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
		//cv::Mat color((int)getColorHeight(), (int)getColorWidth(),CV_8UC3, (void*)frameState.m_colorFrame);
		//cv::imshow("color", color);
		//cv::waitKey(0);
		//cv::Mat depthmat((int)getDepthHeight(), (int)getDepthWidth(),CV_16UC1, (void*)frameState.m_depthFrame);
		//depthmat.convertTo(depthmat, CV_32FC1);
		//cv::normalize(depthmat, depthmat);
		//cv::imshow("color", depthmat *255);
		//cv::waitKey(0);

		int depthsize = getDepthWidth()*getDepthHeight();

		for (unsigned int i = 0; i <depthsize; i++) {
			if (frameState.m_depthFrame[i] == 0) depth[i] = -std::numeric_limits<float>::infinity();
			else depth[i] = (float)frameState.m_depthFrame[i] / m_sensorData->m_depthShift;
		}

		incrementRingbufIdx();

		if (m_bHasColorData) {
			int colorsize= getColorWidth()*getColorHeight();
			for (unsigned int i = 0; i < colorsize; i++) {
				m_colorRGBX[i] = vec4uc(frameState.m_colorFrame[i]);
			}
		}
		//frameState.free();

		m_currFrame++;
		return S_OK;
	}
	else {
		return S_FALSE;
	}
}

std::string SensorDataReaderZhou::getSensorName() const
{
	return m_sensorData->m_sensorName;
}

void SensorDataReaderZhou::releaseData()
{
	m_currFrame = 0;
	m_bHasColorData = false;


	SAFE_DELETE(m_sensorDataCache);

	if (m_sensorData) {
		m_sensorData->free();
		SAFE_DELETE(m_sensorData);
	}
}

#endif
